//
// Created by LiuDongPeng on 2023/11/16.
//

#include "foc.h"


/**
 * @brief Clark transform
 * @param[in]   ia
 * @param[in]   ib
 * @param[in]   ic
 * @param[out]  ialpha
 * @param[out]  ibeta
 */
void foc_clark(float32_t ia, float32_t ib, float32_t ic, float32_t *ialpha, float32_t *ibeta)
{
    if (ialpha == NULL || ibeta == NULL)
        return;

//    *ialpha = (ia * 2 - (ib + ic)) / 3;
//    *ibeta = (ib - ic) * SQRT_3 / 3;

    arm_clarke_f32(ia, ib, ialpha, ibeta);
}


/**
 * @brief Park transform
 * @param[in]   ialpha
 * @param[in]   ibeta
 * @param[in]   sinTheta
 * @param[in]   cosTheta
 * @param[out]  id
 * @param[out]  iq
 */
void foc_park(float32_t ialpha, float32_t ibeta, float32_t sinTheta, float32_t cosTheta, float32_t *id, float32_t *iq)
{
    if (id == NULL || iq == NULL)
        return;

//    *id = ialpha * cosTheta + ibeta * sinTheta;
//    *iq = -ialpha * sinTheta + ibeta * cosTheta;

    arm_park_f32(ialpha, ibeta, id, iq, sinTheta, cosTheta);
}

/**
 * @brief Inv park transform
 * @param[in]   ud
 * @param[in]   uq
 * @param[in]   sinTheta
 * @param[in]   cosTheta
 * @param[out]  ualpha
 * @param[out]  ubeta
 */
void foc_inv_park(float32_t ud, float32_t uq, float32_t sinTheta, float32_t cosTheta, float32_t *ualpha, float32_t *ubeta)
{
    if (ualpha == NULL || ubeta == NULL)
        return;

    *ualpha = ud * cosTheta - uq * sinTheta;
    *ubeta = ud * sinTheta + uq * cosTheta;
}


/**
 * @brief Calc 3 phase pwm duty
 * @param[in]   ualpha
 * @param[in]   ubeta
 * @param[in]   udc
 * @param[in]   tpwm
 * @param[out]  ta
 * @param[out]  tb
 * @param[out]  tc
 */
void foc_svpwm(float32_t ualpha, float32_t ubeta, float32_t udc, float32_t tpwm,
               float32_t *ta, float32_t *tb, float32_t *tc)
{
    if (ta == NULL || tb == NULL || tc == NULL)
        return;

    // 0. 预备工作
	const float32_t Ts = 1.0f;
    const float32_t sqrt3 = SQRT_3;
    const float32_t sqrt3TpwmUdc = sqrt3 * tpwm / udc;

    /* 1. 计算扇区 */
    const int sectorTable[8] = {0, 2, 6, 1, 4, 3, 5, 0};
    int sector = 0;
    int A, B, C, N;

    float32_t u1, u2, u3;
	float ratio = SQRT_3 * Ts / udc; // 调制比
    u1 = ubeta * ratio;
    u2 = (SQRT_3_DIV_2 * ualpha - ubeta * 0.5f) * ratio;
    u3 = (-SQRT_3_DIV_2 * ualpha - ubeta * 0.5f) * ratio;

    A = u1 > 0 ? 1 : 0;
    B = u2 > 0 ? 1 : 0;
    C = u3 > 0 ? 1 : 0;
    N = 4 * C + 2 * B + A;
	N = (u1 > 0.0f) + ((u2 > 0.0f) << 1) + ((u3 > 0.0f) << 2); // sector = A + 2B + 4C
    sector = sectorTable[N];

    // 3. 根据扇区计算三路PWM占空比
	float t1, t2, t3, t4, t5, t6, t7, tSum, kSvpwm;
	float Ta, Tb, Tc;
    switch (N)
    {
        case 3:
            t4 = u2;
			t6 = u1;
			tSum = t4 + t6;
			if (tSum > Ts)
			{
				kSvpwm = Ts / tSum;
				t4 *= kSvpwm;
				t6 *= kSvpwm;
			}
			t7 = (Ts - t4 - t6) / 2.0f;
			Ta = t4 + t6 + t7;
			Tb = t6 + t7;
			Tc = t7;
            break;

        case 1:
	        t2 = -u2;
		    t6 = -u3;
		    tSum = t2 + t6;
		    if (tSum > Ts)
		    {
			    kSvpwm = Ts / tSum;
			    t2 *= kSvpwm;
			    t6 *= kSvpwm;
		    }
		    t7 = (Ts - t2 - t6) / 2.0f;
		    Ta = t6 + t7;
		    Tb = t2 + t6 + t7;
		    Tc = t7;
            break;

        case 5:
	        t2 = u1;
		    t3 = u3;
		    tSum = t2 + t3;
		    if (tSum > Ts)
		    {
			    kSvpwm = Ts / tSum;
			    t2 *= kSvpwm;
			    t3 *= kSvpwm;
		    }
		    t7 = (Ts - t2 - t3) / 2.0f;
		    Ta = t7;
		    Tb = t2 + t3 + t7;
		    Tc = t3 + t7;
            break;

        case 4:
	        t1 = -u1;
		    t3 = -u2;
		    tSum = t1 + t3;
		    if (tSum > Ts)
		    {
			    kSvpwm = Ts / tSum;
			    t1 *= kSvpwm;
			    t3 *= kSvpwm;
		    }
		    t7 = (Ts - t1 - t3) / 2.0f;
		    Ta = t7;
		    Tb = t3 + t7;
		    Tc = t1 + t3 + t7;
            break;

        case 6:
	        t1 = u3;
		    t5 = u2;
		    tSum = t1 + t5;
		    if (tSum > Ts)
		    {
			    kSvpwm = Ts / tSum;
			    t1 *= kSvpwm;
			    t5 *= kSvpwm;
		    }
		    t7 = (Ts - t1 - t5) / 2.0f;
		    Ta = t5 + t7;
		    Tb = t7;
		    Tc = t1 + t5 + t7;
            break;

        case 2:
        default:
	        t4 = -u3;
		    t5 = -u1;
		    tSum = t4 + t5;
		    if (tSum > Ts)
		    {
			    kSvpwm = Ts / tSum;
			    t4 *= kSvpwm;
			    t5 *= kSvpwm;
		    }
		    t7 = (Ts - t4 - t5) / 2.0f;
		    Ta = t4 + t5 + t7;
		    Tb = t7;
		    Tc = t5 + t7;
            break;
    }

    *ta = Ta;
    *tb = Tb;
    *tc = Tc;
}
